package com.jzy.game.engine.math;

import java.io.Serializable;

/**四元数<br>
 * A simple quaternion class.
 * 
 * @see <a href=
 *      "http://en.wikipedia.org/wiki/Quaternion">http://en.wikipedia.org/wiki/Quaternion</a>
 * @author badlogicgames@gmail.com
 * @author vesuvio
 * @author xoppa
 */
public class Quaternion implements Serializable {
	private static final long serialVersionUID = -7661875440774897168L;
	private static Quaternion tmp1 = new Quaternion(0, 0, 0, 0);
	private static Quaternion tmp2 = new Quaternion(0, 0, 0, 0);

	public float x;
	public float y;
	public float z;
	public float w;

	/**
	 * Constructor, sets the four components of the quaternion.
	 * 
	 * @param x
	 *            The x-component
	 * @param y
	 *            The y-component
	 * @param z
	 *            The z-component
	 * @param w
	 *            The w-component
	 */
	public Quaternion(float x, float y, float z, float w) {
		this.set(x, y, z, w);
	}

	public Quaternion() {
		idt();
	}

	/**
	 * Constructor, sets the quaternion components from the given quaternion.
	 * 
	 * @param quaternion
	 *            The quaternion to copy.
	 */
	public Quaternion(Quaternion quaternion) {
		this.set(quaternion);
	}

	/**
	 * Constructor, sets the quaternion from the given axis vector and the angle
	 * around that axis in degrees.
	 * 
	 * @param axis
	 *            The axis
	 * @param angle
	 *            The angle in degrees.
	 */
	public Quaternion(Vector3 axis, float angle) {
		this.set(axis, angle);
	}

	/**
	 * Sets the components of the quaternion
	 * 
	 * @param x
	 *            The x-component
	 * @param y
	 *            The y-component
	 * @param z
	 *            The z-component
	 * @param w
	 *            The w-component
	 * @return This quaternion for chaining
	 */
	public Quaternion set(float x, float y, float z, float w) {
		this.x = x;
		this.y = y;
		this.z = z;
		this.w = w;
		return this;
	}

	/**
	 * Sets the quaternion components from the given quaternion.
	 * 
	 * @param quaternion
	 *            The quaternion.
	 * @return This quaternion for chaining.
	 */
	public Quaternion set(Quaternion quaternion) {
		return this.set(quaternion.x, quaternion.y, quaternion.z, quaternion.w);
	}

	/**
	 * Sets the quaternion components from the given axis and angle around that
	 * axis.
	 * 
	 * @param axis
	 *            The axis
	 * @param angle
	 *            The angle in degrees
	 * @return This quaternion for chaining.
	 */
	public Quaternion set(Vector3 axis, float angle) {
		return setFromAxis(axis.x, axis.y, axis.z, angle);
	}

	/** @return a copy of this quaternion */
	public Quaternion cpy() {
		return new Quaternion(this);
	}

	/** @return the euclidean length of the specified quaternion */
	public final static float len(final float x, final float y, final float z, final float w) {
		return (float) Math.sqrt(x * x + y * y + z * z + w * w);
	}

	/** @return the euclidean length of this quaternion */
	public float len() {
		return (float) Math.sqrt(x * x + y * y + z * z + w * w);
	}

	@Override
	public String toString() {
		return "[" + x + "|" + y + "|" + z + "|" + w + "]";
	}

	/**
	 * Sets the quaternion to the given euler angles in degrees.
	 * 
	 * @param yaw
	 *            the rotation around the y axis in degrees
	 * @param pitch
	 *            the rotation around the x axis in degrees
	 * @param roll
	 *            the rotation around the z axis degrees
	 * @return this quaternion
	 */
	public Quaternion setEulerAngles(float yaw, float pitch, float roll) {
		return setEulerAnglesRad(yaw * MathUtil.degreesToRadians, pitch * MathUtil.degreesToRadians,
				roll * MathUtil.degreesToRadians);
	}

	/**
	 * Sets the quaternion to the given euler angles in radians.
	 * 
	 * @param yaw
	 *            the rotation around the y axis in radians
	 * @param pitch
	 *            the rotation around the x axis in radians
	 * @param roll
	 *            the rotation around the z axis in radians
	 * @return this quaternion
	 */
	public Quaternion setEulerAnglesRad(float yaw, float pitch, float roll) {
		final float hr = roll * 0.5f;
		final float shr = (float) Math.sin(hr);
		final float chr = (float) Math.cos(hr);
		final float hp = pitch * 0.5f;
		final float shp = (float) Math.sin(hp);
		final float chp = (float) Math.cos(hp);
		final float hy = yaw * 0.5f;
		final float shy = (float) Math.sin(hy);
		final float chy = (float) Math.cos(hy);
		final float chy_shp = chy * shp;
		final float shy_chp = shy * chp;
		final float chy_chp = chy * chp;
		final float shy_shp = shy * shp;

		x = (chy_shp * chr) + (shy_chp * shr); // cos(yaw/2) * sin(pitch/2) * cos(roll/2) + sin(yaw/2) * cos(pitch/2) *
												// sin(roll/2)
		y = (shy_chp * chr) - (chy_shp * shr); // sin(yaw/2) * cos(pitch/2) * cos(roll/2) - cos(yaw/2) * sin(pitch/2) *
												// sin(roll/2)
		z = (chy_chp * shr) - (shy_shp * chr); // cos(yaw/2) * cos(pitch/2) * sin(roll/2) - sin(yaw/2) * sin(pitch/2) *
												// cos(roll/2)
		w = (chy_chp * chr) + (shy_shp * shr); // cos(yaw/2) * cos(pitch/2) * cos(roll/2) + sin(yaw/2) * sin(pitch/2) *
												// sin(roll/2)
		return this;
	}

	/**
	 * Get the pole of the gimbal lock, if any.
	 * 
	 * @return positive (+1) for north pole, negative (-1) for south pole, zero (0)
	 *         when no gimbal lock
	 */
	public int getGimbalPole() {
		final float t = y * x + z * w;
		return t > 0.499f ? 1 : (t < -0.499f ? -1 : 0);
	}

	/**
	 * Get the roll euler angle in radians, which is the rotation around the z axis.
	 * Requires that this quaternion is normalized.
	 * 
	 * @return the rotation around the z axis in radians (between -PI and +PI)
	 */
	public float getRollRad() {
		final int pole = getGimbalPole();
		return pole == 0 ? MathUtil.atan2(2f * (w * z + y * x), 1f - 2f * (x * x + z * z))
				: (float) pole * 2f * MathUtil.atan2(y, w);
	}

	/**
	 * Get the roll euler angle in degrees, which is the rotation around the z axis.
	 * Requires that this quaternion is normalized.
	 * 
	 * @return the rotation around the z axis in degrees (between -180 and +180)
	 */
	public float getRoll() {
		return getRollRad() * MathUtil.radiansToDegrees;
	}

	/**
	 * Get the pitch euler angle in radians, which is the rotation around the x
	 * axis. Requires that this quaternion is normalized.
	 * 
	 * @return the rotation around the x axis in radians (between -(PI/2) and
	 *         +(PI/2))
	 */
	public float getPitchRad() {
		final int pole = getGimbalPole();
		return pole == 0 ? (float) Math.asin(MathUtil.clamp(2f * (w * x - z * y), -1f, 1f))
				: (float) pole * MathUtil.PI * 0.5f;
	}

	/**
	 * Get the pitch euler angle in degrees, which is the rotation around the x
	 * axis. Requires that this quaternion is normalized.
	 * 
	 * @return the rotation around the x axis in degrees (between -90 and +90)
	 */
	public float getPitch() {
		return getPitchRad() * MathUtil.radiansToDegrees;
	}

	/**
	 * Get the yaw euler angle in radians, which is the rotation around the y axis.
	 * Requires that this quaternion is normalized.
	 * 
	 * @return the rotation around the y axis in radians (between -PI and +PI)
	 */
	public float getYawRad() {
		return getGimbalPole() == 0 ? MathUtil.atan2(2f * (y * w + x * z), 1f - 2f * (y * y + x * x)) : 0f;
	}

	/**
	 * Get the yaw euler angle in degrees, which is the rotation around the y axis.
	 * Requires that this quaternion is normalized.
	 * 
	 * @return the rotation around the y axis in degrees (between -180 and +180)
	 */
	public float getYaw() {
		return getYawRad() * MathUtil.radiansToDegrees;
	}

	public final static float len2(final float x, final float y, final float z, final float w) {
		return x * x + y * y + z * z + w * w;
	}

	/** @return the length of this quaternion without square root */
	public float len2() {
		return x * x + y * y + z * z + w * w;
	}

	/**
	 * Normalizes this quaternion to unit length
	 * 
	 * @return the quaternion for chaining
	 */
	public Quaternion nor() {
		float len = len2();
		if (len != 0.f && !MathUtil.isEqual(len, 1f)) {
			len = (float) Math.sqrt(len);
			w /= len;
			x /= len;
			y /= len;
			z /= len;
		}
		return this;
	}

	/**
	 * Conjugate the quaternion.
	 * 
	 * @return This quaternion for chaining
	 */
	public Quaternion conjugate() {
		x = -x;
		y = -y;
		z = -z;
		return this;
	}

	// TODO : this would better fit into the vector3 class
	/**
	 * Transforms the given vector using this quaternion
	 * 
	 * @param v
	 *            Vector to transform
	 */
	public Vector3 transform(Vector3 v) {
		tmp2.set(this);
		tmp2.conjugate();
		tmp2.mulLeft(tmp1.set(v.x, v.y, v.z, 0)).mulLeft(this);

		v.x = tmp2.x;
		v.y = tmp2.y;
		v.z = tmp2.z;
		return v;
	}

	/**
	 * Multiplies this quaternion with another one in the form of this = this *
	 * other
	 * 
	 * @param other
	 *            Quaternion to multiply with
	 * @return This quaternion for chaining
	 */
	public Quaternion mul(final Quaternion other) {
		final float newX = this.w * other.x + this.x * other.w + this.y * other.z - this.z * other.y;
		final float newY = this.w * other.y + this.y * other.w + this.z * other.x - this.x * other.z;
		final float newZ = this.w * other.z + this.z * other.w + this.x * other.y - this.y * other.x;
		final float newW = this.w * other.w - this.x * other.x - this.y * other.y - this.z * other.z;
		this.x = newX;
		this.y = newY;
		this.z = newZ;
		this.w = newW;
		return this;
	}

	/**
	 * Multiplies this quaternion with another one in the form of this = this *
	 * other
	 * 
	 * @param x
	 *            the x component of the other quaternion to multiply with
	 * @param y
	 *            the y component of the other quaternion to multiply with
	 * @param z
	 *            the z component of the other quaternion to multiply with
	 * @param w
	 *            the w component of the other quaternion to multiply with
	 * @return This quaternion for chaining
	 */
	public Quaternion mul(final float x, final float y, final float z, final float w) {
		final float newX = this.w * x + this.x * w + this.y * z - this.z * y;
		final float newY = this.w * y + this.y * w + this.z * x - this.x * z;
		final float newZ = this.w * z + this.z * w + this.x * y - this.y * x;
		final float newW = this.w * w - this.x * x - this.y * y - this.z * z;
		this.x = newX;
		this.y = newY;
		this.z = newZ;
		this.w = newW;
		return this;
	}

	/**
	 * Multiplies this quaternion with another one in the form of this = other *
	 * this
	 * 
	 * @param other
	 *            Quaternion to multiply with
	 * @return This quaternion for chaining
	 */
	public Quaternion mulLeft(Quaternion other) {
		final float newX = other.w * this.x + other.x * this.w + other.y * this.z - other.z * y;
		final float newY = other.w * this.y + other.y * this.w + other.z * this.x - other.x * z;
		final float newZ = other.w * this.z + other.z * this.w + other.x * this.y - other.y * x;
		final float newW = other.w * this.w - other.x * this.x - other.y * this.y - other.z * z;
		this.x = newX;
		this.y = newY;
		this.z = newZ;
		this.w = newW;
		return this;
	}

	/**
	 * Multiplies this quaternion with another one in the form of this = other *
	 * this
	 * 
	 * @param x
	 *            the x component of the other quaternion to multiply with
	 * @param y
	 *            the y component of the other quaternion to multiply with
	 * @param z
	 *            the z component of the other quaternion to multiply with
	 * @param w
	 *            the w component of the other quaternion to multiply with
	 * @return This quaternion for chaining
	 */
	public Quaternion mulLeft(final float x, final float y, final float z, final float w) {
		final float newX = w * this.x + x * this.w + y * this.z - z * this.y;
		final float newY = w * this.y + y * this.w + z * this.x - x * this.z;
		final float newZ = w * this.z + z * this.w + x * this.y - y * this.x;
		final float newW = w * this.w - x * this.x - y * this.y - z * this.z;
		this.x = newX;
		this.y = newY;
		this.z = newZ;
		this.w = newW;
		return this;
	}

	/**
	 * Add the x,y,z,w components of the passed in quaternion to the ones of this
	 * quaternion
	 */
	public Quaternion add(Quaternion quaternion) {
		this.x += quaternion.x;
		this.y += quaternion.y;
		this.z += quaternion.z;
		this.w += quaternion.w;
		return this;
	}

	/**
	 * Add the x,y,z,w components of the passed in quaternion to the ones of this
	 * quaternion
	 */
	public Quaternion add(float qx, float qy, float qz, float qw) {
		this.x += qx;
		this.y += qy;
		this.z += qz;
		this.w += qw;
		return this;
	}

	// TODO : the matrix4 set(quaternion) doesnt set the last row+col of the matrix
	// to 0,0,0,1 so... that's why there is this
	// method
	/**
	 * Fills a 4x4 matrix with the rotation matrix represented by this quaternion.
	 * 
	 * @param matrix
	 *            Matrix to fill
	 */
	public void toMatrix(final float[] matrix) {
		final float xx = x * x;
		final float xy = x * y;
		final float xz = x * z;
		final float xw = x * w;
		final float yy = y * y;
		final float yz = y * z;
		final float yw = y * w;
		final float zz = z * z;
		final float zw = z * w;
		// Set matrix from quaternion
		matrix[Matrix4.M00] = 1 - 2 * (yy + zz);
		matrix[Matrix4.M01] = 2 * (xy - zw);
		matrix[Matrix4.M02] = 2 * (xz + yw);
		matrix[Matrix4.M03] = 0;
		matrix[Matrix4.M10] = 2 * (xy + zw);
		matrix[Matrix4.M11] = 1 - 2 * (xx + zz);
		matrix[Matrix4.M12] = 2 * (yz - xw);
		matrix[Matrix4.M13] = 0;
		matrix[Matrix4.M20] = 2 * (xz - yw);
		matrix[Matrix4.M21] = 2 * (yz + xw);
		matrix[Matrix4.M22] = 1 - 2 * (xx + yy);
		matrix[Matrix4.M23] = 0;
		matrix[Matrix4.M30] = 0;
		matrix[Matrix4.M31] = 0;
		matrix[Matrix4.M32] = 0;
		matrix[Matrix4.M33] = 1;
	}

	/**
	 * Sets the quaternion to an identity Quaternion
	 * 
	 * @return this quaternion for chaining
	 */
	public Quaternion idt() {
		return this.set(0, 0, 0, 1);
	}

	/** @return If this quaternion is an identity Quaternion */
	public boolean isIdentity() {
		return MathUtil.isZero(x) && MathUtil.isZero(y) && MathUtil.isZero(z) && MathUtil.isEqual(w, 1f);
	}

	/** @return If this quaternion is an identity Quaternion */
	public boolean isIdentity(final float tolerance) {
		return MathUtil.isZero(x, tolerance) && MathUtil.isZero(y, tolerance) && MathUtil.isZero(z, tolerance)
				&& MathUtil.isEqual(w, 1f, tolerance);
	}

	// todo : the setFromAxis(v3,float) method should replace the set(v3,float)
	// method
	/**
	 * Sets the quaternion components from the given axis and angle around that
	 * axis.
	 * 
	 * @param axis
	 *            The axis
	 * @param degrees
	 *            The angle in degrees
	 * @return This quaternion for chaining.
	 */
	public Quaternion setFromAxis(final Vector3 axis, final float degrees) {
		return setFromAxis(axis.x, axis.y, axis.z, degrees);
	}

	/**
	 * Sets the quaternion components from the given axis and angle around that
	 * axis.
	 * 
	 * @param axis
	 *            The axis
	 * @param radians
	 *            The angle in radians
	 * @return This quaternion for chaining.
	 */
	public Quaternion setFromAxisRad(final Vector3 axis, final float radians) {
		return setFromAxisRad(axis.x, axis.y, axis.z, radians);
	}

	/**
	 * Sets the quaternion components from the given axis and angle around that
	 * axis.
	 * 
	 * @param x
	 *            X direction of the axis
	 * @param y
	 *            Y direction of the axis
	 * @param z
	 *            Z direction of the axis
	 * @param degrees
	 *            The angle in degrees
	 * @return This quaternion for chaining.
	 */
	public Quaternion setFromAxis(final float x, final float y, final float z, final float degrees) {
		return setFromAxisRad(x, y, z, degrees * MathUtil.degreesToRadians);
	}

	/**
	 * Sets the quaternion components from the given axis and angle around that
	 * axis.
	 * 
	 * @param x
	 *            X direction of the axis
	 * @param y
	 *            Y direction of the axis
	 * @param z
	 *            Z direction of the axis
	 * @param radians
	 *            The angle in radians
	 * @return This quaternion for chaining.
	 */
	public Quaternion setFromAxisRad(final float x, final float y, final float z, final float radians) {
		float d = Vector3.len(x, y, z);
		if (d == 0f)
			return idt();
		d = 1f / d;
		float l_ang = radians < 0 ? MathUtil.PI2 - (-radians % MathUtil.PI2) : radians % MathUtil.PI2;
		float l_sin = (float) Math.sin(l_ang / 2);
		float l_cos = (float) Math.cos(l_ang / 2);
		return this.set(d * x * l_sin, d * y * l_sin, d * z * l_sin, l_cos).nor();
	}

	/**
	 * Sets the Quaternion from the given matrix, optionally removing any scaling.
	 */
	public Quaternion setFromMatrix(boolean normalizeAxes, Matrix4 matrix) {
		return setFromAxes(normalizeAxes, matrix.val[Matrix4.M00], matrix.val[Matrix4.M01], matrix.val[Matrix4.M02],
				matrix.val[Matrix4.M10], matrix.val[Matrix4.M11], matrix.val[Matrix4.M12], matrix.val[Matrix4.M20],
				matrix.val[Matrix4.M21], matrix.val[Matrix4.M22]);
	}

	/**
	 * Sets the Quaternion from the given rotation matrix, which must not contain
	 * scaling.
	 */
	public Quaternion setFromMatrix(Matrix4 matrix) {
		return setFromMatrix(false, matrix);
	}

	/**
	 * Sets the Quaternion from the given matrix, optionally removing any scaling.
	 */
	public Quaternion setFromMatrix(boolean normalizeAxes, Matrix3 matrix) {
		return setFromAxes(normalizeAxes, matrix.val[Matrix3.M00], matrix.val[Matrix3.M01], matrix.val[Matrix3.M02],
				matrix.val[Matrix3.M10], matrix.val[Matrix3.M11], matrix.val[Matrix3.M12], matrix.val[Matrix3.M20],
				matrix.val[Matrix3.M21], matrix.val[Matrix3.M22]);
	}

	/**
	 * Sets the Quaternion from the given rotation matrix, which must not contain
	 * scaling.
	 */
	public Quaternion setFromMatrix(Matrix3 matrix) {
		return setFromMatrix(false, matrix);
	}

	/**
	 * <p>
	 * Sets the Quaternion from the given x-, y- and z-axis which have to be
	 * orthonormal.
	 * </p>
	 * 
	 * <p>
	 * Taken from Bones framework for JPCT, see http://www.aptalkarga.com/bones/
	 * which in turn took it from Graphics Gem code at
	 * ftp://ftp.cis.upenn.edu/pub/graphics/shoemake/quatut.ps.Z.
	 * </p>
	 * 
	 * @param xx
	 *            x-axis x-coordinate
	 * @param xy
	 *            x-axis y-coordinate
	 * @param xz
	 *            x-axis z-coordinate
	 * @param yx
	 *            y-axis x-coordinate
	 * @param yy
	 *            y-axis y-coordinate
	 * @param yz
	 *            y-axis z-coordinate
	 * @param zx
	 *            z-axis x-coordinate
	 * @param zy
	 *            z-axis y-coordinate
	 * @param zz
	 *            z-axis z-coordinate
	 */
	public Quaternion setFromAxes(float xx, float xy, float xz, float yx, float yy, float yz, float zx, float zy,
			float zz) {
		return setFromAxes(false, xx, xy, xz, yx, yy, yz, zx, zy, zz);
	}

	/**
	 * <p>
	 * Sets the Quaternion from the given x-, y- and z-axis.
	 * </p>
	 * 
	 * <p>
	 * Taken from Bones framework for JPCT, see http://www.aptalkarga.com/bones/
	 * which in turn took it from Graphics Gem code at
	 * ftp://ftp.cis.upenn.edu/pub/graphics/shoemake/quatut.ps.Z.
	 * </p>
	 * 
	 * @param normalizeAxes
	 *            whether to normalize the axes (necessary when they contain
	 *            scaling)
	 * @param xx
	 *            x-axis x-coordinate
	 * @param xy
	 *            x-axis y-coordinate
	 * @param xz
	 *            x-axis z-coordinate
	 * @param yx
	 *            y-axis x-coordinate
	 * @param yy
	 *            y-axis y-coordinate
	 * @param yz
	 *            y-axis z-coordinate
	 * @param zx
	 *            z-axis x-coordinate
	 * @param zy
	 *            z-axis y-coordinate
	 * @param zz
	 *            z-axis z-coordinate
	 */
	public Quaternion setFromAxes(boolean normalizeAxes, float xx, float xy, float xz, float yx, float yy, float yz,
			float zx, float zy, float zz) {
		if (normalizeAxes) {
			final float lx = 1f / Vector3.len(xx, xy, xz);
			final float ly = 1f / Vector3.len(yx, yy, yz);
			final float lz = 1f / Vector3.len(zx, zy, zz);
			xx *= lx;
			xy *= lx;
			xz *= lx;
			yx *= ly;
			yy *= ly;
			yz *= ly;
			zx *= lz;
			zy *= lz;
			zz *= lz;
		}
		// the trace is the sum of the diagonal elements; see
		// http://mathworld.wolfram.com/MatrixTrace.html
		final float t = xx + yy + zz;

		// we protect the division by s by ensuring that s>=1
		if (t >= 0) { // |w| >= .5
			float s = (float) Math.sqrt(t + 1); // |s|>=1 ...
			w = 0.5f * s;
			s = 0.5f / s; // so this division isn't bad
			x = (zy - yz) * s;
			y = (xz - zx) * s;
			z = (yx - xy) * s;
		} else if ((xx > yy) && (xx > zz)) {
			float s = (float) Math.sqrt(1.0 + xx - yy - zz); // |s|>=1
			x = s * 0.5f; // |x| >= .5
			s = 0.5f / s;
			y = (yx + xy) * s;
			z = (xz + zx) * s;
			w = (zy - yz) * s;
		} else if (yy > zz) {
			float s = (float) Math.sqrt(1.0 + yy - xx - zz); // |s|>=1
			y = s * 0.5f; // |y| >= .5
			s = 0.5f / s;
			x = (yx + xy) * s;
			z = (zy + yz) * s;
			w = (xz - zx) * s;
		} else {
			float s = (float) Math.sqrt(1.0 + zz - xx - yy); // |s|>=1
			z = s * 0.5f; // |z| >= .5
			s = 0.5f / s;
			x = (xz + zx) * s;
			y = (zy + yz) * s;
			w = (yx - xy) * s;
		}

		return this;
	}

	/**
	 * Set this quaternion to the rotation between two vectors.
	 * 
	 * @param v1
	 *            The base vector, which should be normalized.
	 * @param v2
	 *            The target vector, which should be normalized.
	 * @return This quaternion for chaining
	 */
	public Quaternion setFromCross(final Vector3 v1, final Vector3 v2) {
		final float dot = MathUtil.clamp(v1.dot(v2), -1f, 1f);
		final float angle = (float) Math.acos(dot);
		return setFromAxisRad(v1.y * v2.z - v1.z * v2.y, v1.z * v2.x - v1.x * v2.z, v1.x * v2.y - v1.y * v2.x, angle);
	}

	/**
	 * Set this quaternion to the rotation between two vectors.
	 * 
	 * @param x1
	 *            The base vectors x value, which should be normalized.
	 * @param y1
	 *            The base vectors y value, which should be normalized.
	 * @param z1
	 *            The base vectors z value, which should be normalized.
	 * @param x2
	 *            The target vector x value, which should be normalized.
	 * @param y2
	 *            The target vector y value, which should be normalized.
	 * @param z2
	 *            The target vector z value, which should be normalized.
	 * @return This quaternion for chaining
	 */
	public Quaternion setFromCross(final float x1, final float y1, final float z1, final float x2, final float y2,
			final float z2) {
		final float dot = MathUtil.clamp(Vector3.dot(x1, y1, z1, x2, y2, z2), -1f, 1f);
		final float angle = (float) Math.acos(dot);
		return setFromAxisRad(y1 * z2 - z1 * y2, z1 * x2 - x1 * z2, x1 * y2 - y1 * x2, angle);
	}

	/**
	 * Spherical linear interpolation between this quaternion and the other
	 * quaternion, based on the alpha value in the range [0,1]. Taken from Bones
	 * framework for JPCT, see http://www.aptalkarga.com/bones/
	 * 
	 * @param end
	 *            the end quaternion
	 * @param alpha
	 *            alpha in the range [0,1]
	 * @return this quaternion for chaining
	 */
	public Quaternion slerp(Quaternion end, float alpha) {
		final float d = this.x * end.x + this.y * end.y + this.z * end.z + this.w * end.w;
		float absDot = d < 0.f ? -d : d;

		// Set the first and second scale for the interpolation
		float scale0 = 1f - alpha;
		float scale1 = alpha;

		// Check if the angle between the 2 quaternions was big enough to
		// warrant such calculations
		if ((1 - absDot) > 0.1) {// Get the angle between the 2 quaternions,
			// and then store the sin() of that angle
			final float angle = (float) Math.acos(absDot);
			final float invSinTheta = 1f / (float) Math.sin(angle);

			// Calculate the scale for q1 and q2, according to the angle and
			// it's sine value
			scale0 = ((float) Math.sin((1f - alpha) * angle) * invSinTheta);
			scale1 = ((float) Math.sin((alpha * angle)) * invSinTheta);
		}

		if (d < 0.f)
			scale1 = -scale1;

		// Calculate the x, y, z and w values for the quaternion by using a
		// special form of linear interpolation for quaternions.
		x = (scale0 * x) + (scale1 * end.x);
		y = (scale0 * y) + (scale1 * end.y);
		z = (scale0 * z) + (scale1 * end.z);
		w = (scale0 * w) + (scale1 * end.w);

		// Return the interpolated quaternion
		return this;
	}

	/**
	 * Spherical linearly interpolates multiple quaternions and stores the result in
	 * this Quaternion. Will not destroy the data previously inside the elements of
	 * q. result = (q_1^w_1)*(q_2^w_2)* ... *(q_n^w_n) where w_i=1/n.
	 * 
	 * @param q
	 *            List of quaternions
	 * @return This quaternion for chaining
	 */
	public Quaternion slerp(Quaternion[] q) {

		// Calculate exponents and multiply everything from left to right
		final float w = 1.0f / q.length;
		set(q[0]).exp(w);
		for (int i = 1; i < q.length; i++)
			mul(tmp1.set(q[i]).exp(w));
		nor();
		return this;
	}

	/**
	 * Spherical linearly interpolates multiple quaternions by the given weights and
	 * stores the result in this Quaternion. Will not destroy the data previously
	 * inside the elements of q or w. result = (q_1^w_1)*(q_2^w_2)* ... *(q_n^w_n)
	 * where the sum of w_i is 1. Lists must be equal in length.
	 * 
	 * @param q
	 *            List of quaternions
	 * @param w
	 *            List of weights
	 * @return This quaternion for chaining
	 */
	public Quaternion slerp(Quaternion[] q, float[] w) {

		// Calculate exponents and multiply everything from left to right
		set(q[0]).exp(w[0]);
		for (int i = 1; i < q.length; i++)
			mul(tmp1.set(q[i]).exp(w[i]));
		nor();
		return this;
	}

	/**
	 * Calculates (this quaternion)^alpha where alpha is a real number and stores
	 * the result in this quaternion. See
	 * http://en.wikipedia.org/wiki/Quaternion#Exponential.2C_logarithm.2C_and_power
	 * 
	 * @param alpha
	 *            Exponent
	 * @return This quaternion for chaining
	 */
	public Quaternion exp(float alpha) {

		// Calculate |q|^alpha
		float norm = len();
		float normExp = (float) Math.pow(norm, alpha);

		// Calculate theta
		float theta = (float) Math.acos(w / norm);

		// Calculate coefficient of basis elements
		float coeff = 0;
		if (Math.abs(theta) < 0.001) // If theta is small enough, use the limit of sin(alpha*theta) / sin(theta)
										// instead of actual
			// value
			coeff = normExp * alpha / norm;
		else
			coeff = (float) (normExp * Math.sin(alpha * theta) / (norm * Math.sin(theta)));

		// Write results
		w = (float) (normExp * Math.cos(alpha * theta));
		x *= coeff;
		y *= coeff;
		z *= coeff;

		// Fix any possible discrepancies
		nor();

		return this;
	}

	@Override
	public int hashCode() {
		final int prime = 31;
		int result = 1;
		result = prime * result + Float.floatToRawIntBits(w);
		result = prime * result + Float.floatToRawIntBits(x);
		result = prime * result + Float.floatToRawIntBits(y);
		result = prime * result + Float.floatToRawIntBits(z);
		return result;
	}

	@Override
	public boolean equals(Object obj) {
		if (this == obj) {
			return true;
		}
		if (obj == null) {
			return false;
		}
		if (!(obj instanceof Quaternion)) {
			return false;
		}
		Quaternion other = (Quaternion) obj;
		return (Float.floatToRawIntBits(w) == Float.floatToRawIntBits(other.w))
				&& (Float.floatToRawIntBits(x) == Float.floatToRawIntBits(other.x))
				&& (Float.floatToRawIntBits(y) == Float.floatToRawIntBits(other.y))
				&& (Float.floatToRawIntBits(z) == Float.floatToRawIntBits(other.z));
	}

	/**
	 * Get the dot product between the two quaternions (commutative).
	 * 
	 * @param x1
	 *            the x component of the first quaternion
	 * @param y1
	 *            the y component of the first quaternion
	 * @param z1
	 *            the z component of the first quaternion
	 * @param w1
	 *            the w component of the first quaternion
	 * @param x2
	 *            the x component of the second quaternion
	 * @param y2
	 *            the y component of the second quaternion
	 * @param z2
	 *            the z component of the second quaternion
	 * @param w2
	 *            the w component of the second quaternion
	 * @return the dot product between the first and second quaternion.
	 */
	public final static float dot(final float x1, final float y1, final float z1, final float w1, final float x2,
			final float y2, final float z2, final float w2) {
		return x1 * x2 + y1 * y2 + z1 * z2 + w1 * w2;
	}

	/**
	 * Get the dot product between this and the other quaternion (commutative).
	 * 
	 * @param other
	 *            the other quaternion.
	 * @return the dot product of this and the other quaternion.
	 */
	public float dot(final Quaternion other) {
		return this.x * other.x + this.y * other.y + this.z * other.z + this.w * other.w;
	}

	/**
	 * Get the dot product between this and the other quaternion (commutative).
	 * 
	 * @param x
	 *            the x component of the other quaternion
	 * @param y
	 *            the y component of the other quaternion
	 * @param z
	 *            the z component of the other quaternion
	 * @param w
	 *            the w component of the other quaternion
	 * @return the dot product of this and the other quaternion.
	 */
	public float dot(final float x, final float y, final float z, final float w) {
		return this.x * x + this.y * y + this.z * z + this.w * w;
	}

	/**
	 * Multiplies the components of this quaternion with the given scalar.
	 * 
	 * @param scalar
	 *            the scalar.
	 * @return this quaternion for chaining.
	 */
	public Quaternion mul(float scalar) {
		this.x *= scalar;
		this.y *= scalar;
		this.z *= scalar;
		this.w *= scalar;
		return this;
	}

	/**
	 * Get the axis angle representation of the rotation in degrees. The supplied
	 * vector will receive the axis (x, y and z values) of the rotation and the
	 * value returned is the angle in degrees around that axis. Note that this
	 * method will alter the supplied vector, the existing value of the vector is
	 * ignored.
	 * </p>
	 * This will normalize this quaternion if needed. The received axis is a unit
	 * vector. However, if this is an identity quaternion (no rotation), then the
	 * length of the axis may be zero.
	 * 
	 * @param axis
	 *            vector which will receive the axis
	 * @return the angle in degrees
	 * @see <a href=
	 *      "http://en.wikipedia.org/wiki/Axis%E2%80%93angle_representation">wikipedia</a>
	 * @see <a href=
	 *      "http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToAngle">calculation</a>
	 */
	public float getAxisAngle(Vector3 axis) {
		return getAxisAngleRad(axis) * MathUtil.radiansToDegrees;
	}

	/**
	 * Get the axis-angle representation of the rotation in radians. The supplied
	 * vector will receive the axis (x, y and z values) of the rotation and the
	 * value returned is the angle in radians around that axis. Note that this
	 * method will alter the supplied vector, the existing value of the vector is
	 * ignored.
	 * </p>
	 * This will normalize this quaternion if needed. The received axis is a unit
	 * vector. However, if this is an identity quaternion (no rotation), then the
	 * length of the axis may be zero.
	 * 
	 * @param axis
	 *            vector which will receive the axis
	 * @return the angle in radians
	 * @see <a href=
	 *      "http://en.wikipedia.org/wiki/Axis%E2%80%93angle_representation">wikipedia</a>
	 * @see <a href=
	 *      "http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToAngle">calculation</a>
	 */
	public float getAxisAngleRad(Vector3 axis) {
		if (this.w > 1)
			this.nor(); // if w>1 acos and sqrt will produce errors, this cant happen if quaternion is
						// normalised
		float angle = (float) (2.0 * Math.acos(this.w));
		double s = Math.sqrt(1 - this.w * this.w); // assuming quaternion normalised then w is less than 1, so term
													// always positive.
		if (s < MathUtil.FLOAT_ROUNDING_ERROR) { // test to avoid divide by zero, s is always positive due to sqrt
			// if s close to zero then direction of axis not important
			axis.x = this.x; // if it is important that axis is normalised then replace with x=1; y=z=0;
			axis.y = this.y;
			axis.z = this.z;
		} else {
			axis.x = (float) (this.x / s); // normalise axis
			axis.y = (float) (this.y / s);
			axis.z = (float) (this.z / s);
		}

		return angle;
	}

	/**
	 * Get the angle in radians of the rotation this quaternion represents. Does not
	 * normalize the quaternion. Use {@link #getAxisAngleRad(Vector3)} to get both
	 * the axis and the angle of this rotation. Use
	 * {@link #getAngleAroundRad(Vector3)} to get the angle around a specific axis.
	 * 
	 * @return the angle in radians of the rotation
	 */
	public float getAngleRad() {
		return (float) (2.0 * Math.acos((this.w > 1) ? (this.w / len()) : this.w));
	}

	/**
	 * Get the angle in degrees of the rotation this quaternion represents. Use
	 * {@link #getAxisAngle(Vector3)} to get both the axis and the angle of this
	 * rotation. Use {@link #getAngleAround(Vector3)} to get the angle around a
	 * specific axis.
	 * 
	 * @return the angle in degrees of the rotation
	 */
	public float getAngle() {
		return getAngleRad() * MathUtil.radiansToDegrees;
	}

	/**
	 * Get the swing rotation and twist rotation for the specified axis. The twist
	 * rotation represents the rotation around the specified axis. The swing
	 * rotation represents the rotation of the specified axis itself, which is the
	 * rotation around an axis perpendicular to the specified axis.
	 * </p>
	 * The swing and twist rotation can be used to reconstruct the original
	 * quaternion: this = swing * twist
	 * 
	 * @param axisX
	 *            the X component of the normalized axis for which to get the swing
	 *            and twist rotation
	 * @param axisY
	 *            the Y component of the normalized axis for which to get the swing
	 *            and twist rotation
	 * @param axisZ
	 *            the Z component of the normalized axis for which to get the swing
	 *            and twist rotation
	 * @param swing
	 *            will receive the swing rotation: the rotation around an axis
	 *            perpendicular to the specified axis
	 * @param twist
	 *            will receive the twist rotation: the rotation around the specified
	 *            axis
	 * @see <a href=
	 *      "http://www.euclideanspace.com/maths/geometry/rotations/for/decomposition">calculation</a>
	 */
	public void getSwingTwist(final float axisX, final float axisY, final float axisZ, final Quaternion swing,
			final Quaternion twist) {
		final float d = Vector3.dot(this.x, this.y, this.z, axisX, axisY, axisZ);
		twist.set(axisX * d, axisY * d, axisZ * d, this.w).nor();
		if (d < 0)
			twist.mul(-1f);
		swing.set(twist).conjugate().mulLeft(this);
	}

	/**
	 * Get the swing rotation and twist rotation for the specified axis. The twist
	 * rotation represents the rotation around the specified axis. The swing
	 * rotation represents the rotation of the specified axis itself, which is the
	 * rotation around an axis perpendicular to the specified axis.
	 * </p>
	 * The swing and twist rotation can be used to reconstruct the original
	 * quaternion: this = swing * twist
	 * 
	 * @param axis
	 *            the normalized axis for which to get the swing and twist rotation
	 * @param swing
	 *            will receive the swing rotation: the rotation around an axis
	 *            perpendicular to the specified axis
	 * @param twist
	 *            will receive the twist rotation: the rotation around the specified
	 *            axis
	 * @see <a href=
	 *      "http://www.euclideanspace.com/maths/geometry/rotations/for/decomposition">calculation</a>
	 */
	public void getSwingTwist(final Vector3 axis, final Quaternion swing, final Quaternion twist) {
		getSwingTwist(axis.x, axis.y, axis.z, swing, twist);
	}

	/**
	 * Get the angle in radians of the rotation around the specified axis. The axis
	 * must be normalized.
	 * 
	 * @param axisX
	 *            the x component of the normalized axis for which to get the angle
	 * @param axisY
	 *            the y component of the normalized axis for which to get the angle
	 * @param axisZ
	 *            the z component of the normalized axis for which to get the angle
	 * @return the angle in radians of the rotation around the specified axis
	 */
	public float getAngleAroundRad(final float axisX, final float axisY, final float axisZ) {
		final float d = Vector3.dot(this.x, this.y, this.z, axisX, axisY, axisZ);
		final float l2 = Quaternion.len2(axisX * d, axisY * d, axisZ * d, this.w);
		return MathUtil.isZero(l2) ? 0f
				: (float) (2.0
						* Math.acos(MathUtil.clamp((float) ((d < 0 ? -this.w : this.w) / Math.sqrt(l2)), -1f, 1f)));
	}

	/**
	 * Get the angle in radians of the rotation around the specified axis. The axis
	 * must be normalized.
	 * 
	 * @param axis
	 *            the normalized axis for which to get the angle
	 * @return the angle in radians of the rotation around the specified axis
	 */
	public float getAngleAroundRad(final Vector3 axis) {
		return getAngleAroundRad(axis.x, axis.y, axis.z);
	}

	/**
	 * Get the angle in degrees of the rotation around the specified axis. The axis
	 * must be normalized.
	 * 
	 * @param axisX
	 *            the x component of the normalized axis for which to get the angle
	 * @param axisY
	 *            the y component of the normalized axis for which to get the angle
	 * @param axisZ
	 *            the z component of the normalized axis for which to get the angle
	 * @return the angle in degrees of the rotation around the specified axis
	 */
	public float getAngleAround(final float axisX, final float axisY, final float axisZ) {
		return getAngleAroundRad(axisX, axisY, axisZ) * MathUtil.radiansToDegrees;
	}

	/**
	 * Get the angle in degrees of the rotation around the specified axis. The axis
	 * must be normalized.
	 * 
	 * @param axis
	 *            the normalized axis for which to get the angle
	 * @return the angle in degrees of the rotation around the specified axis
	 */
	public float getAngleAround(final Vector3 axis) {
		return getAngleAround(axis.x, axis.y, axis.z);
	}
}
